#include "Arduino.h"
#include "CryptStream.h"
#include "xtea.h"

//#define DEBUG 1

CryptStream::CryptStream(const uint32_t key[4]) {
  instart = 0;
  outstart = 0;
  _available = false;
  _overflow = false;
  _key = key;
}

void CryptStream::writebyte(uint8_t c) {
  /* Low level function to put a single byte (uint8_t) of data in the input stream */
  
  if (_overflow) return;
  
  instream[instart++] = c;

  #ifdef DEBUG
  Serial.print("CryptStream::writebyte: written ");Serial.print(c);Serial.print("\tstart: ");Serial.println(instart);
  #endif
  
  if (instart == BLOCKSIZE) { // input stream full: will be encrypted
  
    if (_available) {
      #ifdef DEBUG
      Serial.println("CryptStream::writebyte: OVERFLOW");
      #endif
      _overflow = true;
      return;
    }

    memcpy(outstream, instream, sizeof(instream)); // move instream to outstream as is, because encrypt() will operate in place
    encrypt((uint32_t*)outstream, _key);
    instart = 0;
    _available = true;
    
    #ifdef DEBUG
    Serial.println("CryptStream::writebyte: OUTPUT BUFFER READY");
    Serial.print("instream:  ");
    for (uint8_t i=0; i<BLOCKSIZE; i++) { Serial.print(instream[i], HEX); Serial.print(" "); }
    Serial.println();
    Serial.print("outstream: ");
    for (uint8_t i=0; i<BLOCKSIZE; i++) { Serial.print(outstream[i], HEX); Serial.print(" "); }
    Serial.println();
    #endif
    
  }
  
};

void CryptStream::finalize() {
  /* Function to finalize a stream.
   * Must be called after the last write() in order to fill the stream with the needed
   * padding bytes. Padding method is PKCS7 as described in RFC5652.
   */
  
  uint8_t padding = BLOCKSIZE - instart;
  #ifdef DEBUG
  Serial.print("CryptStream::finalize: padding = ");Serial.println(padding);
  #endif
  while (!_available) writebyte(padding);
}

uint8_t CryptStream::read() {
  /* Function to read the output stream one byte at a time.
   * Should be called only if available() is true.
   */
  
  if (!_available) return 0;
  
  uint8_t ret = outstream[outstart++];
  
  if (outstart == BLOCKSIZE) { // output stream empty
    outstart = 0;
    _available = false;
  }

  return ret;
  
}

bool CryptStream::available() {
  /* Return true if the output stream contains a whole block.
   * In such case, one will want to read() the whole output buffer before furthermore writings
   * in order to avoid an overflow.
   */
  return _available;
}

bool CryptStream::overflow() {
  /* Return true if writings were attempted while both input and output streams were full.
   * Note that this means that the stream is corrupted.
   */
  return _overflow;
}

